﻿// 测试程序，用于验证opencv模板匹配在图像定位质量上的应用可行性。 2024/7/25
//

#include <iostream>
#include <string>
#include "opencv2/core.hpp"
#include "opencv2/imgproc.hpp"
#include "opencv2/highgui.hpp"
#include <opencv2/features2d.hpp>
#include <vector>
#include <sstream>
#include <fstream>
#include "gdal_priv.h"

using namespace std;
using namespace cv;

struct RasterMat {
    RasterMat();
    double trans[6];
    double centerLon;
    double centerLat;
    int xsize;
    int ysize;
    Mat matData;
    bool loadFromFile(string filename, double scale255min, double scale255max);
};

RasterMat::RasterMat() {
    trans[0] = 0; trans[1] = 0; trans[2] = 0; trans[3] = 0;
    trans[4] = 0; trans[5] = 0;
    centerLon = -999;
    centerLat = -999;
    xsize = 0;
    ysize = 0;
}
//从文件加载栅格数据到Mat结构体
bool RasterMat::loadFromFile(string filename, double scale255min, double scale255max)
{
    GDALDataset* ds = (GDALDataset*)GDALOpen(filename.c_str(), GA_ReadOnly);
    if (ds == 0) {
        cout << "Error, failed to read raster " << filename << endl;
        return false;
    }
    ds->GetGeoTransform(trans);
    this->xsize = ds->GetRasterXSize();
    this->ysize = ds->GetRasterYSize();
    this->centerLat = trans[3] + trans[5] * (this->ysize / 2 + 0.5 );
    this->centerLon = trans[0] + trans[1] * (this->xsize / 2 + 0.5);
    double diff = scale255max - scale255min  ;
    double step = 255 / diff;
    vector<int> clipdata(this->ysize * this->xsize);
    ds->GetRasterBand(1)->RasterIO(GF_Read, 0, 0, this->xsize, this->ysize, clipdata.data(),
        this->xsize, this->ysize, GDT_Int32, 0, 0, 0);
    matData = Mat(this->ysize, this->xsize, CV_8UC1);
    for (int iy = 0; iy < this->ysize; ++iy) {
        for (int ix = 0; ix < this->xsize; ++ix) {
            int v = clipdata[iy * this->xsize + ix];
            if (v < scale255min) v = 0;
            else if (v > scale255max) v = 255;
            else {
                v = (v - scale255min) * step;
                v = fmax(v, 0);
                v = fmin(v, 255);
            }
            matData.at<unsigned char>(iy, ix) = (unsigned char)v;
        }
    }
    GDALClose(ds); ds = 0;
    return true;
}

// 根据控制点中心经纬度和半径，在input图像中切割出一个缓冲半径为10个像素的稍大一片（相对控制点片）。
void cutBiggerPiece(RasterMat& inMat, RasterMat& gcpMat, RasterMat& retBiggerMat) {
    double clon = gcpMat.centerLon;
    double clat = gcpMat.centerLat;

    int gcpCenterInX = (clon - inMat.trans[0]) / inMat.trans[1];
    int gcpCenterInY = (clat - inMat.trans[3]) / inMat.trans[5];
    int radius2x = gcpMat.xsize / 2 + 10;
    int radius2y = gcpMat.ysize / 2 + 10;

    retBiggerMat.trans[0] = inMat.trans[0] + inMat.trans[1] * (gcpCenterInX - radius2x);
    retBiggerMat.trans[1] = inMat.trans[1];
    retBiggerMat.trans[2] = inMat.trans[2];
    retBiggerMat.trans[3] = inMat.trans[3] + inMat.trans[5] * (gcpCenterInY - radius2y);
    retBiggerMat.trans[4] = inMat.trans[4];
    retBiggerMat.trans[5] = inMat.trans[5];

    retBiggerMat.xsize = radius2x * 2 + 1;
    retBiggerMat.ysize = radius2y * 2 + 1;

    retBiggerMat.centerLat = gcpMat.centerLat;
    retBiggerMat.centerLon = gcpMat.centerLon;

    retBiggerMat.matData = Mat(radius2y * 2 + 1, radius2x * 2 + 1, CV_8UC1);
    for (int iy = -radius2y; iy <= radius2y; ++iy) {
        for (int ix = -radius2x; ix <= radius2x; ++ix) {
            int innx = gcpCenterInX + ix;
            int inny = gcpCenterInY + iy;
            if (innx >= 0 && innx < inMat.xsize && inny >= 0 && inny < inMat.ysize) {
                retBiggerMat.matData.at<unsigned char>(iy + radius2y, ix + radius2x) = inMat.matData.at<unsigned char>(inny, innx);
            }
            else {
                retBiggerMat.matData.at<unsigned char>(iy+ radius2y, ix+ radius2x) = 0;
            }
        }
    }
}

// 使用gcp作为模板，在bigger上面查找最接近的匹配区域，然后返回偏差，
// 如果最大相关系数小于 0.6 那么跳过不做判断。
bool matchBestPoint(RasterMat& bigger, RasterMat& gcp, double& errorXInPixel, double& errorYInPixel) {
    Mat result;
    int resultXsize = bigger.xsize - gcp.xsize + 1;
    int resultYsize = bigger.ysize - gcp.ysize + 1;
    result.create(resultYsize, resultXsize, CV_32FC1);
    cv::matchTemplate(bigger.matData, gcp.matData, result, cv::TM_CCORR_NORMED );
    double mincc = 0; 
    double maxcc = 0;
    cv::Point minLoc, maxLoc;
    cv::minMaxLoc(result, &mincc, &maxcc, &minLoc, &maxLoc);
    if (maxcc < 0.6) {
        return false;
    }
    else {
        errorXInPixel = maxLoc.x - resultXsize / 2;
        errorYInPixel = maxLoc.y - resultYsize / 2;
        return true;
    }
}

struct WPointError {
    WPointError();
    int x;
    int y;
    int gcpIndex;
};
WPointError::WPointError() { x = 0; y = 0; gcpIndex = 0; }


int main()
{
    std::cout << "A test program for clip matching."<<endl ;
    cout << "version v1.0.0.0 " << endl;
    GDALAllRegister();
    string inputFilename = "test-offset-input2.tif";// "E:/data/MODIS/MOD13C2/sr/mod13c2_20210501_nir.tif";
    string outputFilename = "output.txt";
    string outputJpg = outputFilename + ".jpg";
    double scale255Min = 0;
    double scale255Max = 4000;


    vector<string> clipFileArr;
    vector<RasterMat> clipArray;
    for (int ic = 1; ic <= 9; ++ic) {
        char buff[128];
        sprintf(buff, "gcp%02d.tif", ic);
        string clipfilename(buff);
        clipFileArr.push_back(string(buff));

        RasterMat gc;
        bool gcok = gc.loadFromFile(clipfilename, scale255Min, scale255Max);
        clipArray.push_back(gc);
    }

    RasterMat inputMat;
    bool inok = inputMat.loadFromFile(inputFilename, scale255Min, scale255Max);

    cout << "computing..." << endl;

    vector<RasterMat> biggerPieceArray;
    vector< WPointError> errorPixels;
    double avgErrorInPixels = 0;
    int    numMatched = 0;
    for (int ic = 0; ic < clipArray.size(); ++ic) {
        RasterMat big;
        cutBiggerPiece(inputMat, clipArray[ic], big);
        biggerPieceArray.push_back(big);

        double errorxpx = 0;
        double errorypx = 0;
        bool mok = matchBestPoint(big, clipArray[ic], errorxpx, errorypx);
        if (mok) {
            WPointError wp;
            wp.x = errorxpx;
            wp.y = errorypx;
            wp.gcpIndex = ic;
            errorPixels.push_back(wp);

            ++numMatched;
            avgErrorInPixels += sqrt(errorxpx * errorxpx + errorypx * errorypx);
        }
    }
    cout << "number of good match: " << numMatched << endl;
    if (numMatched > 0) {
        //绘制点
        avgErrorInPixels = avgErrorInPixels / numMatched;
        Mat outputMat;
        cv::cvtColor(inputMat.matData, outputMat, cv::COLOR_GRAY2RGB);
        for (int ip = 0; ip < errorPixels.size(); ++ip) {
            double gcplon = clipArray[errorPixels[ip].gcpIndex].centerLon;
            double gcplat = clipArray[errorPixels[ip].gcpIndex].centerLat;
            int gcpX = (gcplon - inputMat.trans[0]) / inputMat.trans[1];
            int gcpY = (gcplat - inputMat.trans[3]) / inputMat.trans[5];

            int innX = gcpX + errorPixels[ip].x;
            int innY = gcpY + errorPixels[ip].y;
            //draw reference point
            cv::drawMarker(outputMat, cv::Point(gcpX, gcpY), CV_RGB(0,255,0) , cv::MARKER_SQUARE, 10, 1, 8);
            cv::circle(outputMat, cv::Point(gcpX,gcpY), 0.5, CV_RGB(0, 255, 0), 1,8 );
            //draw input point
            cv::drawMarker(outputMat, cv::Point(innX, innY), CV_RGB(255, 0, 0), cv::MARKER_CROSS, 5, 1, 8);
        }

        //legend
        {
            //background
            cv::rectangle(outputMat , cv::Point(15, 15), cv::Point(80, 60), CV_RGB(0,0,0) ,-1 );
            //reference
            cv::drawMarker(outputMat, cv::Point(30, 30), CV_RGB(0, 255, 0), cv::MARKER_SQUARE, 10, 1, 8);
            cv::circle(outputMat, cv::Point(30, 30), 0.5, CV_RGB(0, 255, 0), 1, 8);
            cv::putText(outputMat, "REF", cv::Point(40, 35), 1, 0.8, CV_RGB(255, 255, 255));
            //draw input point
            cv::drawMarker(outputMat, cv::Point(30, 50), CV_RGB(255, 0, 0), cv::MARKER_CROSS, 5, 1, 8);
            cv::putText(outputMat, "INPUT", cv::Point(40, 55), 1, 0.8, CV_RGB(255, 255, 255));
        }
        

        cv::imwrite(outputJpg.c_str(), outputMat);
        //another png for much clear and sharp imagery.
        string pngfilename = outputFilename + ".png";
        cv::imwrite(pngfilename.c_str(), outputMat);
    }
    else {
        avgErrorInPixels = -999 ;//no matchings
    }
    cout << "average of error in pixels: " << avgErrorInPixels << endl;

    cout << "done" << endl;
    return 0;
}
